/* Copyright (c) 2010 Fabian Kulman
* 
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* 
* The above copyright notice and this permission notice shall be included in
* all copies or substantial portions of the Software.
* 
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

#include <iostream>

#include "unit.h"

using namespace std;

// Default velocity and position solver from the library. Improvements can be done here. 
void bodyUpdateVelocityImproved(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt){
	body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
	body->w = body->w*damping + body->t*body->i_inv*dt;
}

void bodyUpdatePositionImproved(cpBody *body, cpFloat dt){
	body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt));
	cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt);

	body->v_bias = cpvzero;
	body->w_bias = 0.0f;
}

cpBodyVelocityFunc velocityFunc = bodyUpdateVelocityImproved;
cpBodyPositionFunc positionFunc = bodyUpdatePositionImproved;



Unit::Unit( cpSpace* space, cpVect pos ):space(space),
		body(NULL),shape(NULL),joint(NULL),constraint(NULL),
		staticBody(NULL),staticShape(NULL),staticJoint(NULL),
		state(1.0),
		lu(NULL),ru(NULL){ 
	
	// Creating the body
	body = cpBodyNew(INFINITY, INFINITY);
	body->p = pos;


	// Creating the shape
	shape = cpPolyShapeNew(body, num, verts, cpvzero);
	shape->e = 0.0f; 
	shape->u = 0.8f;
	cpSpaceAddStaticShape(space, shape);


	//
	id = shape->hashid;
}

Unit::Unit( cpSpace* space, Unit* lu, cpFloat angle ):space(space),
		body(NULL),shape(NULL),joint(NULL),constraint(NULL),
		staticBody(NULL),staticShape(NULL),staticJoint(NULL),
		state(1.0),
		lu(lu),ru(NULL){ 

	// Calculating the position offset
	cpVect jpos1 = {0.5*SCALE, 0.0};							// Vector from center to right joint
	jpos1 = cpvrotate(jpos1, cpvforangle(lu->body->a));	

	cpVect jpos2 = {0.5*SCALE, 0.0};							// Vector from right joint to new position
	jpos2 = cpvrotate(jpos2, cpvforangle(lu->body->a + angle));

	cpVect pos = cpvadd( jpos2, cpvadd( lu->body->p, jpos1 )); 


	// Creating the body
	body = cpBodyNew(UNIT_MASS, cpMomentForPoly(UNIT_MOMENTUM, num, verts, cpvzero));
	cpBodySetAngle(body, lu->body->a + angle);
	body->p = pos;
	body->position_func = positionFunc;
	body->velocity_func = velocityFunc;
	cpSpaceAddBody(space, body);


	// Creating the shape
	shape = cpPolyShapeNew(body, num, verts, cpvzero);
	shape->e = 0.0f; 
	shape->u = 0.8f;
	id = shape->hashid;
	shape->group = id;
	cpSpaceAddShape(space, shape);


	// Creating the joint
	joint = (cpPivotJoint*)cpPivotJointNew(lu->body, body, cpvadd( lu->body->p, jpos1));
	constraint = (cpRotaryLimitJoint*)cpRotaryLimitJointNew( lu->body, body, angle, angle );
	cpSpaceAddConstraint( space, (cpConstraint*)joint );		
	cpSpaceAddConstraint( space, (cpConstraint*)constraint );	


	//
	lookJoint();
	lookPosition();
}

Unit::~Unit(){
	if(lu){
		cpSpaceRemoveShape( space , shape );
		//cpBodyFree(body);
		cpShapeFree(shape);
	}
	else{
		cpSpaceRemoveStaticShape( space, shape );
		cpShapeFree(shape);
	}

	if(staticShape){
		cpSpaceRemoveStaticShape( space, staticShape );
		cpShapeFree(staticShape);
	}
}

void Unit::updateLocalAngle(){
	if(lu){
		localAngle = body->a - lu->body->a;
		state = angleToState( localAngle );
	}
}

void Unit::lookJoint(){
	if(lu){
		updateLocalAngle();
		constraint->min = localAngle;
		constraint->max = localAngle;
	}
}

void Unit::unlookJoint(){
	if(lu){
		updateLocalAngle();
		constraint->min = -1.0472; 
		constraint->max = 2.0944;
	}
}

bool Unit::snap(){
	bool hasSnapped = false;
	if(lu){
		updateLocalAngle();
		if( localAngle > 1.8 ){ 
			constraint->min = 2.0943;
			hasSnapped = true;
		}
		else if( localAngle < -0.8 ){ 
			constraint->max = -1.0471;
			hasSnapped = true;
		}
	}
	return hasSnapped;
}

void Unit::lookPosition(){
	staticBody = cpBodyNew(INFINITY, INFINITY);
	staticBody->p = body->p;

	staticShape = cpCircleShapeNew(staticBody, 1.0f, cpv(0.0, 0.0));
	staticShape->e = 1.0f; staticShape->u = 1.0f;
	staticShape->layers = NOT_GRABABLE_MASK;
	staticShape->group = shape->group;

	staticShape = cpSpaceAddStaticShape(space, staticShape);

	staticJoint = (cpPivotJoint*)cpPivotJointNew(body, staticBody, body->p);
	cpSpaceAddConstraint( space, (cpConstraint*)staticJoint );	
}

void Unit::unlookPosition(){
	cpSpaceRemoveStaticShape( space, staticShape );
	cpSpaceRemoveConstraint( space, (cpConstraint*)staticJoint );		

	cpBodyFree(staticBody);
	cpShapeFree(staticShape);
	cpConstraintFree((cpConstraint*)staticJoint);

	staticShape = NULL;
	staticBody = NULL;
	staticJoint = NULL;
}

void Unit::stopUnit(){
	if(lu){
		updateLocalAngle();
		body->v = cpvzero;
		body->f = cpvzero;
		body->w = 0.0f;
		body->t = 0.0f;
	}
}

void Unit::drawUnit( GLfloat guiScale ){

	// Draw shape 
	cpPolyShape* poly = (cpPolyShape*)shape;

	int count = count=poly->numVerts;
	GLfloat* VAR = (GLfloat*)malloc(sizeof(GLfloat)*(count*2));
	glVertexPointer(2, GL_FLOAT, 0, VAR);

	cpVect *verts = poly->verts;
	for(int i=0; i<count; i++){
		cpVect v = cpvadd(body->p, cpvrotate(verts[i], body->rot));
		VAR[2*i    ] = v.x;
		VAR[2*i + 1] = v.y;
	}

	if(!poly->shape.sensor){
		float brightness = 255.0;
		glColor4f(1.0f, 0.647f, 0.0f, 1.0f);
		glDrawArrays(GL_TRIANGLE_FAN, 0, count);
	}

	// Draw edges
	glLineWidth(3.5f / guiScale);
	glColor3f(0, 0, 0);
	glDrawArrays(GL_LINE_LOOP, 0, count);

	free(VAR);


	// Draw static joint
	//if(staticBody){
	//glColor3f(0.0f, 1.0f, 0.0f);
	//glPointSize(10.0f);
	//glBegin(GL_POINTS); 
	//{
	//	glVertex2f(staticBody->p.x, staticBody->p.y);
	//} 
	//glEnd();
	//}


	// Draw joint
	if(lu){
		cpVect c = cpvadd(body->p, cpvrotate(joint->anchr2, body->rot));
		glColor3f(0.0f, 0.0f, 0.0f);
		glPointSize(10.0f / guiScale);
		glBegin(GL_POINTS); 
		{
			glVertex2f(c.x, c.y);
		} 
		glEnd();

		// Draw joint in invalid state
		if( state != 1.0 && state != 0.0 ){
			glColor3f(1.0f, 0.0f, 0.0f);
			glPointSize(5.0f / guiScale);
			glBegin(GL_POINTS); 
			{
				glVertex2f(c.x, c.y);
			} 
			glEnd();
		}
	}
}

